#ifndef PlanePostProcessMatcher_H_
#define PlanePostProcessMatcher_H_
//OpenCV
#include "cv.h"
#include "highgui.h"
#include <string>
#include <iostream>
#include <stdio.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/common/transformation_from_correspondences.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/thread/thread.hpp>

using namespace std;

class PlanePostProcessMatcher: public FrameMatcher
{
	public:
		FrameMatcher * base;
		PlanePostProcessMatcher();
		PlanePostProcessMatcher(FrameMatcher * base_);
		~PlanePostProcessMatcher();
		Transformation * getTransformation(RGBDFrame * src, RGBDFrame * dst);
		void update();
		Eigen::Matrix4d optimize(vector<segmentation_fit *> match_src, vector<segmentation_fit *> match_dst,Transformation * trans);
		double getError(Eigen::Matrix4d transformationMat,vector<segmentation_fit *> match_src, vector<segmentation_fit *> match_dst,Transformation * trans);
		double * getGradient(Eigen::Matrix4d transformationMat,vector<segmentation_fit *> match_src, vector<segmentation_fit *> match_dst,Transformation * trans);
		Eigen::Matrix4d useGradient(Eigen::Matrix4d transformationMat,double * gradient);
};

#endif
